/********************************************************
 *      This file contains the required routines 		*
 *      required to communicate with the CC2420			*
 ********************************************************/

#include    <system.h>
#include    "cc2420.h"
#include 	<rand.h>
#include	"EESD.h"


int BE, NB, counter;
bit failure, send_side;
volatile bit  CSMA_int@INTCON.2;
volatile bit  FIFOP_left@INTCON.1;
volatile bit  FIFOP_right@INTCON3.1;


volatile bit mssp@PIR1.3; 	// SSPIF: MSSP Interrupt Flag --> trans/recp complete or waiting
bit l_receive = 0; //semaphore for the left chipcon receiving a packet
bit r_receive = 0; //semaphore for the right chipcon receiving a packet

void interrupt(void)
{	
	
	if (FIFOP_left == 1)
	{
		FIFOP_left = 0;
		l_receive = 1;
	}
	
    if (FIFOP_right == 1)
	{
	    FIFOP_right = 0;
		r_receive = 1;
		
	}
	/*	
	if (CSMA_int == 1)
	{
		counter++;
		CSMA_int = 0;			 //clear interrupt flag		
		CSMATMR	= 0; // Timer0 interrupt DISABLED
		
		if(send_side == 0)
		{
			CSMA_CA_l();
		}
		if(send_side == 1)
		{
			CSMA_CA_r();
		} 
	}*/
	
	return;
}	

void SPI_init(void)
{
	
	adcon1 = 0x0f;  // Set all I/O pins to digital
	trisd.0 = 0;	// RESET the chipcon
	latd.0 = 0; 
	delay_ms(250); 
	latd.0 = 1;
	

// INTCON: Interrupt Control Register
	intcon.7 = 0;		// GIE: Global Interrupt Enable 
	intcon.6 = 0;		// PEIE: Peripheral Interrupt Enable 

// PIE1.3 Peripheral Interrupt Enable Register
	pie1.3 = 0;			// SSPIE: MSSP Interrupt Enable 

// IPR1.3: Peripheral Interrupt Priority Register
	ipr1.3 = 0;			// SSPIP: MSSP Interrupt Priority

// SSPCON1 control register for SPI mode
	sspcon1.7 = 0;		// WCOL: Write Collision Detect 
	sspcon1.6 = 0;		// SSPOV: Receive Overflow Indicator --> Slave Mode Only
	sspcon1.5 = 1; 		// SSPEN: Sync. Serial Port Enable 
	sspcon1.4 = 0; 		// CKP: Clock Polarity Select
	sspcon1.3 = 0; 		// SSPM (3:0) Mode Select --> 0010 = FOSC/64
	sspcon1.2 = 0;
	sspcon1.1 = 1;
	sspcon1.0 = 0;
	
// SSPSTAT status register for SPI mode
	sspstat.7 = 1; 		// SMP: Sample bit 
	sspstat.6 = 1; 		// CKE: SPI Clock Select bit
 
	
	trisc.5 = 0; // SDO, serial data out 
	trisc.3 = 0; // SCK, serial clock 
	trisc.4 = 1; // SDI, serial data in 
	
	trisd.1 = 0; // Voltage regulator
	latd.1 = 1; 
	
	trisd.2 = 0; // CSn_l (chip select)
	latd.2 = 1;
	trisd.3 = 0; // CSn_r (chip select)
	latd.3 = 1;

	
// FIFOs and the rest as input pins

	trisb.1 = 1; // FIFO_l
	trisb.0 = 1; // FIFOP_l
	trisb.6 = 1; // CCA_l

	trisb.4 = 1; // FIFO_r
	trisb.2 = 1; // FIFOP_r
	trisb.7 = 1; // CCA_r
	
	//debug
	trisa.5 = 0;
	
	return;
}

void CC2420_init(void)
{
	unsigned short ans;
	
	
	putstring("\r\nRight Status: "); // Configures right CC2420
	CSN_r = 0;
	ans = SSPsend(SNOP);
	ans = SSPsend(SXOSCON);
		puthex(ans);
	ans = SSPsend(STXCAL);
		puthex(ans);
	ans = SSPsend(SRXON);
		puthex(ans);
	ans = SSPsend(SNOP);	// Expected state: 0x46
		puthex(ans);
	CSN_r = 1;
	
	ans = read_reg_r(RXFIFO_REG);
	CSN_r = 0;
	ans = SSPsend(SFLUSHRX);
	ans = SSPsend(SFLUSHRX);
	CSN_r = 1;
	
	
	putstring("\r\nLeft Status: "); // Configures left CC2420
	CSN_l = 0;
	ans = SSPsend(SNOP);
	ans = SSPsend(SXOSCON);
		puthex(ans);
	ans = SSPsend(STXCAL);
		puthex(ans);
	ans = SSPsend(SRXON);
		puthex(ans);
	ans = SSPsend(SNOP);	// Expected state: 0x46
		puthex(ans);
	CSN_l = 1;
	
	ans = read_reg_l(RXFIFO_REG); // Reads RXFIFO before flushing it (refer to CC2420 datasheet)
	CSN_l = 0;
	ans = SSPsend(SFLUSHRX);
	ans = SSPsend(SFLUSHRX);
	CSN_l = 1;
	
	
	write_reg_l(FSCTRL_REG, 0x6565); 	// Set to Channel 11
	write_reg_l(IOCFG0_REG, 25);		// Set FIFOP threshold length
	write_reg_r(FSCTRL_REG, 0x6565); 	
	write_reg_r(IOCFG0_REG, 25);		
}

void intrpt_init(void)
{
	// INTCON bits
	intcon.7 = 1; // Global Interrupt enabled
	intcon.6 = 1; // Peripheral Interrupt enabled
	intcon.5 = 0; // Timer0 interrupt DISABLED
	intcon.4 = 1; // External Interrupt 0 bit enabled
	
	// INTCON2 bit
	intcon2.6 = 1; // External Int 0 on rising edge(FIFOP_l)
	intcon2.4 = 1; // External Int 2 on rising edge(FIFOP_r)
	
	// INTCON3 bit
	intcon3.4 = 1; // External Interrupt 2 bit enabled

	
	// TIMER0 used as CSMA timer
	t0con.7 = 1;
	t0con.6 = 1; // Timer0 set to be 8-bit
	t0con.5 = 0;
	t0con.3 = 0;
	t0con.2 = 1; // prescale factor 1:256
	t0con.1 = 1;
	t0con.0 = 1;
	
	return;
}

/*********************************
 * 	SPI read/write routine  	 *
 *********************************/   
char SSPsend(char address)
{
	sspbuf = address; 	// load data
	while(mssp == 0){}	// wait until transmition is complete
	mssp = 0;			// reset flag
	
	return sspbuf;
}

/*********************************
 * 	Reads an 8-bit register 	 *
 *********************************/
unsigned short read_reg_l(unsigned short address)
{
	unsigned short data;
	char temp;
	
	address = address | 01000000b; // 6-bit address and read command 
	CSN_l = 0;			// chip select
	temp = SSPsend(address);	
	temp = SSPsend(0);
	data = temp;
	data = data << 8;	// Shift data 
	temp = SSPsend(0);
	data = data | temp;
	CSN_l = 1; 		//chip select high
	
	return data;
}
unsigned short read_reg_r(unsigned short address)
{
	unsigned short data;
	char temp;
	
	address = address | 01000000b; // 6-bit address and read command 
	CSN_r = 0;			// chip select
	temp = SSPsend(address);	
	temp = SSPsend(0);
	data = temp;
	data = data << 8;	// Shift data 
	temp = SSPsend(0);
	data = data | temp;
	CSN_r = 1; 		//chip select high
	
	return data;
}


/*********************************
 * 	Writes to an 8-bit register	 *
 *********************************/
void write_reg_l(unsigned short address, unsigned short data)
{
	char temp;
	unsigned short data1, data2;
	
	data1 = ((data >> 8) & 0xFF);
	data2 = (data & 0xFF);	// Separates 16-bit data to 2 8-bit
	CSN_l = 0;				// chip select
	temp = SSPsend(address);	
	temp = SSPsend(data1);
	temp = SSPsend(data2);
	CSN_l = 1; 			//chip select high
	
	return;
}
void write_reg_r(unsigned short address, unsigned short data)
{
	char temp;
	unsigned short data1, data2;
	
	data1 = ((data >> 8) & 0xFF);
	data2 = (data & 0xFF);	// Separates 16-bit data to 2 8-bit
	CSN_r = 0;				// chip select
	temp = SSPsend(address);	
	temp = SSPsend(data1);
	temp = SSPsend(data2);
	CSN_r = 1; 			//chip select high
	
	return;
}

/*********************************
 * 	Reads message from RXFIFO	 *
 *********************************/
char* read_rxfifo_l(void)
{

	unsigned short n, null, length;
	unsigned short address, rssi, fcs;
	unsigned long sfd;
	char msg[19];
	
	
	address = 0x3f | 01000000b;
	
	
	CSN_l = 0;	// chip select
	null = SSPsend(address);
	length = SSPsend(0);
	msg[0] = length;
	n=1;

	if(length == 17)
	{
		for( n=1; n<=17; n++)
		{
			msg[n] = SSPsend(0);
		}
		CSN_l = 1; //chip select high
		
	}
	else	// If the packet is not of the desired length then ignore
	{
		CSN_l = 1; //chip select high
		
		for( n=1; n<=17; n++)
		{
			msg[n] = 0x0E;
		}
	}
	
	return msg;
}
char* read_rxfifo_r(void)
{

	unsigned short n, null, length;
	unsigned short address, temp;
	unsigned long sfd;
	char msg[19];
	
	
	address = 0x3f | 01000000b;
	

	CSN_r = 0;	// chip select
	null = SSPsend(address);
	length = SSPsend(0);
	msg[0] = length;
	n=1;
	
	if(length == 17)
	{
		for( n=1; n<=17; n++)
		{
			msg[n] = SSPsend(0);
		}
		CSN_r = 1; //chip select high
		
	}
	else
	{
		CSN_r = 1; //chip select high
		
		for( n=1; n<=17; n++)
		{
			msg[n] = 0xEE;
		}
	}
	
	
	
	return msg;
}

/*********************************
 * 	Receives packet and beacon,  *
 *  type, RSSI and timestamp     * 
 *  information                  *
 *********************************/
char* receive_pkt_r(void)
{

	unsigned short  payload, length, rssi;
	unsigned short rssi_val, beacon_type, beacon_id, timestamp;
	unsigned long sfd;
	char packet[4];
	char* msg;
	
	msg = read_rxfifo_r();
	length = msg[0];
	payload = length - 10;
	rssi = length-1;
	
	beacon_type = msg[payload+5];	
	beacon_id = msg[payload+3];
	timestamp = msg[payload+7];
	
		// Get signed 2's comp RSSI value (assumption: packet is 15 bytes long)
	rssi_val = msg[rssi];			
	
	if (rssi_val >= 128)
	{
		rssi_val = 256 - rssi_val;
		rssi_val = rssi_val + 45;  // dB offset: -45
	}
	else
	{
		// dB offset: -45
		rssi_val = 45 - rssi_val;
	}
	
	
	if (length != 17)
	{
		beacon_type = 0xEE;
		beacon_id = 0x0;
		timestamp = 0x0;
		rssi_val = 0;
	}
	
	packet[0] = beacon_id;
	packet[1] = beacon_type;
	packet[2] = timestamp;
	packet[3] = rssi_val;			
	
	
	return packet;
}

char* receive_pkt_l(void)
{

	unsigned short  payload, length, rssi;
	unsigned short rssi_val, beacon_type, beacon_id, timestamp;
	unsigned long sfd;
	char packet[4];
	char* msg;
	
	msg = read_rxfifo_l();
	length = msg[0];
	payload = length - 10;
	rssi = length-1;
	
	beacon_id = msg[payload+3];
	beacon_type = msg[payload+5];
	timestamp = msg[payload+7];
	
		// Get signed 2's comp RSSI value (assumption: packet is 15 bytes long)
	rssi_val = msg[rssi];			
	
	if (rssi_val >= 128)
	{
		rssi_val = 256 - rssi_val;
		rssi_val = rssi_val + 45;  // dB offset: -45
	}
	else
	{
		// dB offset: -45
		rssi_val = 45 - rssi_val;
	}
	
	if (length != 17)
	{
		beacon_type = 0xEE;
		beacon_id = 0;
		timestamp = 0;
		rssi_val = 0;
	}
	
	packet[0] = beacon_id;
	packet[1] = beacon_type;
	packet[2] = timestamp;
	packet[3] = rssi_val;			
	
	
	return packet;
}

/*********************************
 * 	Writes message from TXFIFO	 *
 *********************************/
 
void write_txfifo_l(char* payload)
{
	unsigned short length, txfifo_ad, null;
	int n;
	
	txfifo_ad = 0x3E;
	length = payload[0];
	
	CSN_l = 0;
	null = SSPsend(txfifo_ad);
	n=0;
	while(n<=length-2)
	//for( n=0; n<length; n++)
	{
		null = SSPsend(payload[n]);
		n++;
	}
	
	CSN_l = 1;
	
	return;
	
}

void write_txfifo_r(char* payload)
{
	unsigned short length, txfifo_ad, null;
	int n;
	
	txfifo_ad = 0x3E;
	length = payload[0];
	
	CSN_r = 0;
	null = SSPsend(txfifo_ad);
	n=0;
	while(n<=length-2)
	//for( n=0; n<length; n++)
	{
		null = SSPsend(payload[n]);
		n++;
	}
	
	CSN_r = 1;
	
	return;
	
}

/*********************************
 * 	Reads message from TXFIFO	 *
 *********************************/
char* read_txfifo_l(void)
{
	unsigned short length, txfifo_ad, null;
	int n;
	char msg[19];
	
	txfifo_ad = 0x3E | 01000000b;
	
	CSN_l = 0;
	null = SSPsend(txfifo_ad);
	length = SSPsend(SNOP);
	msg[0] = length;
	n=0;
	
	for( n=1; n<=17; n++)
		{
		msg[n] = SSPsend(0);
		n++;
		}
	
	CSN_l = 1;
		
	return msg;
	
}

char* read_txfifo_r(void)
{
	unsigned short length, txfifo_ad, null;
	int n;
	char msg[19];
	
	txfifo_ad = 0x3E | 01000000b;
	
	CSN_r = 0;
	null = SSPsend(txfifo_ad);
	length = SSPsend(SNOP);
	msg[0] = length;
	n=0;
	
	for( n=1; n<=17; n++)
		{
		msg[n] = SSPsend(0);
		n++;
		}
	
	CSN_r = 1;
		
	return msg;
	
}


/*********************************
 * 	Sends the repel packets		 *
 *********************************/
void send_packet_l(char* packet)
{
		
	failure = 0;
	counter = 0; 
	BE = BEinit;
	NB = NBinit;
	
	send_side = 0; // Sending on the left
	
	write_txfifo_l(packet);
	CSMA_CA_l();
	
	return;
}

void send_packet_r(char* packet)
{
		
	failure = 0;
	counter = 0; 
	BE = BEinit;
	NB = NBinit;
	
	send_side = 1; // Sending on the right
	
	write_txfifo_r(packet);
	CSMA_CA_r();
	
	return;
}

/*********************************
 * Algorithm that assures		 *
 * compliance with IEEE 802.15.4 *
 * (CSMA)						 *
 *********************************/
void CSMA_CA_l(void)
{
	unsigned short state;
	bit sent;
	int delay;
	
	if (counter == 0)
	{
		delay = rand() % ((1 << BE) - 1);
		timer_320us(delay);
	}
	
	else
	{
		CSN_l = 0;
		state = SSPsend(STXONCCA);
		state = SSPsend(SNOP);
		CSN_l = 1;
		sent = (state >> 3);
		putstring("\r\n State: ");
		puthex(state);
	
		if (!sent)
		{
			delay = rand() % ((1 << BE) - 1);
			NB = NB + 1;
						
			if (BE == BEMax);
			else BE++;
			
			if (NB > NBMax) 
			{
				failure = 1;
				putstring(" FAILURE\r\n"); //START CHECKING HERE
			}
			else timer_320us(delay);
		}
	}
	return;
}


void CSMA_CA_r(void)
{
	unsigned short state;
	bit sent;
	int delay;
	
	if (counter == 0)
	{
		delay = rand() % ((1 << BE) - 1);
		timer_320us(delay);
	}
	
	else
	{
		CSN_r = 0;
		state = SSPsend(STXONCCA);
		state = SSPsend(SNOP);
		CSN_r = 1;
		sent = (state >> 3);
		putstring("\r\n State: ");
		puthex(state);
	
		if (!sent)
		{
			delay = rand() % ((1 << BE) - 1);
			NB = NB + 1;
						
			if (BE == BEMax);
			else BE++;
			
			if (NB > NBMax) 
			{
				failure = 1;
				putstring(" FAILURE\r\n"); //START CHECKING HERE
			}
			else timer_320us(delay);
		}
	}
	return;
}

void timer_320us(int delay)
{
	tmr0l = -3*delay;
	
	putstring(" TMR0L: ");
	puthex(tmr0l);
	putc(' ');
	putint(delay);

	CSMATMR	 = 1; // Timer0 interrupt ENABLED
	
	return;
	
}
	
	
	